/* Copyright (c) 2011 Alexandre Défossez, Timothée Lacroix
 * This file is part of HINAPT.
 *
 * HINAPT is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HINAPT is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HINAPT.  If not, see <http://www.gnu.org/licenses/>.
 */


#include "oflow.hpp"

bool trackMulti(vector<Point2f>& toTrack,Mat& imgA,Mat& imgB,
                int profMax, int n, int w, double threshold){
  //S'il n'y a rien à faire, on s'arrête dès maintenant
  if (toTrack.size()==0)
    return true;
  Mat* imgsA = new Mat[profMax+1];
  Mat* imgsB = new Mat[profMax+1];
  Vec2d speed;
  //zone limite pour pouvoir suivre les points : le bord de l'image est interdit
  double border = (1<<profMax)*w;
  //on précalcule les images de l'approche pyramidale
  imgsA[0] = imgA;
  imgsB[0] = imgB;
  for(int i=1; i<=profMax; i++){
    pyramidize(imgsA[i-1], imgsA[i]);
    pyramidize(imgsB[i-1], imgsB[i]);
  }
  bool inbound = true;

  #pragma omp parallel for private(speed) reduction(&:inbound)
  for(unsigned int i=0;i<toTrack.size();i++){
    speed=opticalFlowSimple(imgsA,imgsB,toTrack[i].x,toTrack[i].y,
                            profMax, n, w, threshold);
    toTrack[i].x += speed[0];
    toTrack[i].y += speed[1];
    inbound &= !(toTrack[i].x<border || toTrack[i].y<border ||
         toTrack[i].x>imgA.cols-border ||
         toTrack[i].y>imgA.rows-border);
  }
  delete[] imgsA;
  delete[] imgsB;
  return inbound;
}

void calcG(Mat& img, Mat& g, double xp, double yp, int w){
    Mat G(2,2, CV_64FC1, Scalar(0.));
    for(double x=xp-w; x<=xp+w; x++){
      for(double y=yp-w; y<=yp+w; y++){
        Vec2d grad = gradient<uchar>(img, x, y);
        Mat gradM = Mat(2, 1, CV_64FC1);
        gradM.at<double>(0, 0) = grad[0];
        gradM.at<double>(1, 0) = grad[1];
        G += gradM * gradM.t();
      }
    }
    G.copyTo(g);
}

Vec2d opticalFlowSimple(Mat imgsA[], Mat imgsB[],
                        double xp, double yp, int profMax,
                        int n, int w, double threshold, int prof){
  Vec2d g(0, 0);

  Mat& imgA = imgsA[prof];
  Mat& imgB = imgsB[prof];
  int largeur=imgA.cols;
  int hauteur=imgA.rows;
  //On fait une approximation sur une image
  //de taille divisée par 2 (obtenue par pyramidize)
  //On gagne ainsi la possibilitée de tracker
  //des mouvements d'amplitude supèrieure
  if(prof!=profMax){
    g=opticalFlowSimple(imgsA, imgsB, xp/2., yp/2.,
                        profMax, n, w, threshold, prof+1);
  }
  Mat v(2, 1, CV_64FC1, Scalar(0.));
  v.at<double>(0,0)=g[0];
  v.at<double>(1,0)=g[1];
  double deltaV = INFINI;
  Mat G, GInv;
  calcG(imgA, G, xp, yp, w);
  GInv = G.inv();

  //on calcul le vecteur vitesse par itérations successives
  for(int i=0; i<n && deltaV>threshold; i++){
    Vec2d mismatch(0.,0.);
    for(double x=xp-w; x<=xp+w; x++){
      for(double y=yp-w; y<=yp+w; y++){
        double nx=x+v.at<double>(0,0);
        double ny=y+v.at<double>(1,0);
        //on vérifie que les points sont dans l'image
        if(x>0 && x<largeur && y>0 && y<hauteur &&
           nx>0 && nx<largeur && ny>0 && ny<hauteur){
          Vec2d grad = gradient<uchar>(imgA, x, y);
          double delta = getBrightness<uchar>(imgA,x,y)-getBrightness<uchar>(imgB,nx,ny);
          mismatch += delta*grad;
        }
      }
    }
    Mat mismatchM(2, 1, CV_64FC1);
    mismatchM.at<double>(0, 0) = mismatch[0];
    mismatchM.at<double>(1, 0) = mismatch[1];
    Mat deltaVMat = GInv*mismatchM;
    v += deltaVMat;
    deltaVMat = deltaVMat.t() * deltaVMat;
    deltaV = deltaVMat.at<double>(0,0);
  }
  //Si on était dans un niveau de récursion > 0 ,
  //il faut doubler le vecteur vitesse, sinon non
  if(prof>0)
    return Vec2d(2.*v.at<double>(0,0),2.*v.at<double>(1,0));
  else
    return Vec2d(v.at<double>(0,0),v.at<double>(1,0));
}
